#pragma once
#include "GePoint3.h"
//
namespace tge{

	template<typename RealT,typename VectorType,typename PointType>
	class TGeMatrix3
	{
		typedef RealT value_type;
	public:
		value_type entry[4][4];
	public:
		TGeMatrix3(void)
		{
			setToIdentity();
		}
		~TGeMatrix3(void)
		{
		}
		TGeMatrix3& setToIdentity()
		{
			entry[0][0]=1.0;
			entry[0][1]=0.0;
			entry[0][2]=0.0;
			entry[0][3]=0.0;

			entry[1][0]=0.0;
			entry[1][1]=1.0;
			entry[1][2]=0.0;
			entry[1][3]=0.0;

			entry[2][0]=0.0;
			entry[2][1]=0.0;
			entry[2][2]=1.0;
			entry[2][3]=0.0;

			entry[3][0]=0.0;
			entry[3][1]=0.0;
			entry[3][2]=0.0;
			entry[3][3]=1.0;
			return *this;
		}

		TGeMatrix3& setCoordSystem( 
			const PointType&origin
			,const VectorType&ax
			,const VectorType&ay
			,const VectorType&az )
		{
			entry[0][3]=origin.x;
			entry[1][3]=origin.y;
			entry[2][3]=origin.z;
			entry[3][3]=1.0;

			entry[0][0]=ax.x;
			entry[1][0]=ax.y;
			entry[2][0]=ax.z;
			entry[3][0]=0.0;

			entry[0][1]=ay.x;
			entry[1][1]=ay.y;
			entry[2][1]=ay.z;
			entry[3][1]=0.0;

			entry[0][2]=az.x;
			entry[1][2]=az.y;
			entry[2][2]=az.z;
			entry[3][2]=0.0;
			return *this;
		}

		TGeMatrix3& getCoordSystem( PointType &origin, VectorType &ax, VectorType &ay, VectorType &az ) 
		{
			origin.set(entry[0][3],entry[1][3],entry[2][3]);
			ax.set(entry[0][0],entry[1][0],entry[2][0]);
			ay.set(entry[0][1],entry[1][1],entry[2][1]);
			az.set(entry[0][2],entry[1][2],entry[2][2]);
			return *this;
		}
		TGeMatrix3&invert()
		{
			dcinv((value_type*)entry,1.0e-12);
			return*this;
		}
		bool dcinv(value_type *a,value_type tol) 
		{
			int i,j,k,l,u,v;
			value_type d,p;
			const int n=4;
			int is[4],js[4];
			for(k=0;k<n;k++)
			{
				d=0.0;
				for(i=k;i<n;i++)
					for(j=k;j<n;j++)
					{
						l=i*n+j;
						p=fabs(a[l]);
						if(p>d){d=p;is[k]=i;js[k]=j;}
					}
					if(d<tol)
					{
						return FALSE;
					}
					if(is[k]!=k)
						for(j=0;j<n;j++)
						{
							u=k*n+j;
							v=is[k]*n+j;
							p=a[u];a[u]=a[v];a[v]=p;
						}
						if(js[k]!=k)
							for(i=0;i<n;i++)
							{
								u=i*n+k;v=i*n+js[k];
								p=a[u];a[u]=a[v];a[v]=p;
							}
							l=k*n+k;
							a[l]=1.0/a[l];
							for(j=0;j<n;j++)
								if(j!=k)
								{u=k*n+j;a[u]=a[u]*a[l];}
								for(i=0;i<n;i++)
									if(i!=k)
										for(j=0;j<n;j++)
											if(j!=k)
											{
												u=i*n+j;
												a[u]=a[u]-a[i*n+k]*a[k*n+j];
											}
											for(i=0;i<n;i++)
												if(i!=k)
												{u=i*n+k;a[u]=-a[u]*a[l];}
			}
			for(k=n-1;k>=0;k--)
			{
				if(js[k]!=k)
				{
					for(j=0;j<n;j++)
					{
						u=k*n+j;v=js[k]*n+j;
						p=a[u];a[u]=a[v];a[v]=p;
					}
				}
				if(is[k]!=k)
				{
					for(i=0;i<n;i++)
					{
						u=i*n+k;v=i*n+is[k];
						p=a[u];a[u]=a[v];a[v]=p;
					}
				}
			}
			return true;
		}
		TGeMatrix3& setToScale(value_type scale,const PointType&center=PointType())
		{
			value_type s=1-scale;
			entry[0][0]=scale;
			entry[0][1]=0.0;
			entry[0][2]=0.0;
			entry[0][3]=s*center.x;

			entry[1][0]=0.0;
			entry[1][1]=scale;
			entry[1][2]=0.0;
			entry[1][3]=s*center.y;

			entry[2][0]=0.0;
			entry[2][1]=0.0;
			entry[2][2]=scale;
			entry[2][3]=s*center.z;

			entry[3][0]=0;
			entry[3][1]=0;
			entry[3][2]=0;
			entry[3][3]=1.0;
			return *this;
		}
		TGeMatrix3& setToTranslation(const VectorType &vec)
		{
			entry[0][0]=1.0;
			entry[0][1]=0.0;
			entry[0][2]=0.0;
			entry[0][3]=vec.x;
			//
			entry[1][0]=0.0;
			entry[1][1]=1.0;
			entry[1][2]=0.0;
			entry[1][3]=vec.y;
			//
			entry[2][0]=0.0;
			entry[2][1]=0.0;
			entry[2][2]=1.0;
			entry[2][3]=vec.z;
			//
			entry[3][0]=0.0;
			entry[3][1]=0.0;
			entry[3][2]=0.0;
			entry[3][3]=1.0;
			return *this;
		}
		TGeMatrix3& setToRotation(
			value_type angle, 
			const VectorType &axis, 
			const PointType &center=PointType())
		{
			//
			value_type len=axis.length();
			angle/=2;
			value_type sina,cosa;
			sina=sin(angle);
			cosa=cos(angle);
			value_type a=sina*axis.x/len;
			value_type b=sina*axis.y/len;
			value_type c=sina*axis.z/len;
			TGeMatrix3 tm2;
			tm2.entry[0][0]=1.0-2*b*b-2*c*c;
			tm2.entry[0][1]=2*a*b-2*cosa*c;
			tm2.entry[0][2]=2*a*c+2*cosa*b;
			tm2.entry[0][3]=0;

			tm2.entry[1][0]=2*a*b+2*cosa*c;
			tm2.entry[1][1]=1.0-2*a*a-2*c*c;
			tm2.entry[1][2]=2*b*c-2*cosa*a;
			tm2.entry[1][3]=0;

			tm2.entry[2][0]=2*a*c-2*cosa*b;
			tm2.entry[2][1]=2*b*c+2*cosa*a;
			tm2.entry[2][2]=1.0-2*a*a-2*b*b;
			tm2.entry[2][3]=0;
			//
			TGeMatrix3 tm1;
			tm1.setToTranslation(center.asVector());
			TGeMatrix3 tm3;
			tm3.setToTranslation(-center.asVector());
			tm1.postMultBy(tm2);
			tm1.postMultBy(tm3);
			*this=tm1;
			return *this;
		}
		TGeMatrix3& TGeMatrix3::preMultBy(const TGeMatrix3 &leftSide)
		{
			value_type tmp[4][4];
			int r,c;
			for(r=0;r<4;++r)
			{
				for(c=0;c<4;++c)
				{
					tmp[r][c]=
						leftSide.entry[r][0]*entry[0][c]+
						leftSide.entry[r][1]*entry[1][c]+
						leftSide.entry[r][2]*entry[2][c]+
						leftSide.entry[r][3]*entry[3][c];
				}
			}
			memcpy(entry,tmp,sizeof(TGeMatrix3));
			return *this;
		}
		TGeMatrix3& postMultBy(const TGeMatrix3 &rightSide)
		{
			double tmp[4][4];
			int r,c;
			for(r=0;r<4;++r)
			{
				for(c=0;c<4;++c)
				{
					tmp[r][c]=
						entry[r][0]*rightSide.entry[0][c]+
						entry[r][1]*rightSide.entry[1][c]+
						entry[r][2]*rightSide.entry[2][c]+
						entry[r][3]*rightSide.entry[3][c];
				}
			}
			memcpy(entry,tmp,sizeof(TGeMatrix3));
			return *this;
		}

		TGeMatrix3& setToScale(value_type sx, value_type sy, value_type sz, const PointType&center=PointType())
		{
			entry[0][0]=sx;
			entry[0][1]=0.0;
			entry[0][2]=0.0;
			entry[0][3]=(1-sx)*center.x;

			entry[1][0]=0.0;
			entry[1][1]=sy;
			entry[1][2]=0.0;
			entry[1][3]=(1-sy)*center.y;

			entry[2][0]=0.0;
			entry[2][1]=0.0;
			entry[2][2]=sz;
			entry[2][3]=(1-sz)*center.z;

			entry[3][0]=0;
			entry[3][1]=0;
			entry[3][2]=0;
			entry[3][3]=1.0;
			return *this;
		}
		friend PointType operator*(const PointType&pt,const TGeMatrix3&tm)
		{
			value_type x=pt.x;
			value_type y=pt.y;
			value_type z=pt.z;
			value_type xx=x*tm.entry[0][0]+y*tm.entry[0][1]+z*tm.entry[0][2]+tm.entry[0][3];
			value_type yy=x*tm.entry[1][0]+y*tm.entry[1][1]+z*tm.entry[1][2]+tm.entry[1][3];
			value_type zz=x*tm.entry[2][0]+y*tm.entry[2][1]+z*tm.entry[2][2]+tm.entry[2][3];
			return PointType(xx,yy,zz);
		}
	};
	template<typename RealT>
	TGePoint3<RealT>&TGePoint3<RealT>::transformBy(const MatrixType&tm)
	{
		value_type xx=x*tm.entry[0][0]+y*tm.entry[0][1]+z*tm.entry[0][2]+tm.entry[0][3];
		value_type yy=x*tm.entry[1][0]+y*tm.entry[1][1]+z*tm.entry[1][2]+tm.entry[1][3];
		value_type zz=x*tm.entry[2][0]+y*tm.entry[2][1]+z*tm.entry[2][2]+tm.entry[2][3];
		x=xx;
		y=yy;
		z=zz;
		return *this;
	}
	template<typename RealT>
	TGeVector3<RealT>&TGeVector3<RealT>::transformBy(const MatrixType&tm)
	{
		RealT xx=x*tm.entry[0][0]+y*tm.entry[0][1]+z*tm.entry[0][2];
		RealT yy=x*tm.entry[1][0]+y*tm.entry[1][1]+z*tm.entry[1][2];
		RealT zz=x*tm.entry[2][0]+y*tm.entry[2][1]+z*tm.entry[2][2];
		x=xx;
		y=yy;
		z=zz;
		return *this;
	}
	template<typename RealT>
	TGeVector3<RealT>& TGeVector3<RealT>::rotateBy(RealT angle ,const TGeVector3& axis )
	{
		MatrixType tm;
		tm.setToRotation(angle,axis);
		transformBy(tm);
		return (*this);
	}
	typedef TGeMatrix3<double,GeVector3d,GePoint3d>GeMatrix3d;
}